/// //////////////////////////////////////////////
///@file  计算相机到路面目标的距离
///@brief 需要标定相机 相机内参 
///@version v1.0
///@date  2020.06.09
/// //////////////////////////////////////////////

#include <iostream> 
#include <fstream> 
#include "eigen3/Eigen/Dense"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include "yaml-cpp/yaml.h"

/// 将相机的像素坐标转化成世界坐标
 class Coordinate_ransformation
 {
public:
    //相机内参数矩阵 
    cv::Mat cameraMatrix; 
    //摄像机的5个畸变系数：k1,k2,p1,p2,k3 
    cv::Mat distCoeffs; 
    //实际坐标系的距离，若工作平面与相机距离固定可设置为0
    double zConst;
    //相机外参旋转矩阵
    cv::Mat rotationMatrix;
    //相机外参平移向量
    cv::Mat tvec;
    //相机对车身坐标Y轴补偿值
    double Y_offset;
    //相机对车身坐标X轴补偿值
    double X_offset;

private:
    cv::Point3f getWorldPoints(cv::Point2f inPoints);

public:
    Coordinate_ransformation();

    cv::Point3d getfloorPoints(cv::Point inPoints);

    cv::Point2f getUVPoints(cv::Point3d worldPoint);

    // bool LoadIntrinsic(const std::string &file_path) ;

    // bool LoadExternal(const std::string &file_path) ;

    void Calculate_target_distance(cv::Point2f inPoints_left , cv::Point2f inPoints_right, double *longitudinal,double *lateral,double *veh_width);
 };
